
#include "gskeleton.h"
#include <queue>
#include <algorithm>

GSkeleton::GSkeleton()
   :Skeleton() {
   
   int njoints = 50;
   closest_vertex.reserve(njoints);
   min_distance_vertex.reserve(njoints);
   onlyvisible.reserve(njoints);
   d.reserve(njoints);
   k.reserve(njoints);
   kdist.reserve(njoints);
   visited.reserve(njoints);
   D.reserve(njoints);
}

GSkeleton::GSkeleton(Skeleton &s)
   :Skeleton(s) {
   
   graph = s.getUndirectedGraph();
   genBonesList();
   joints.resize(nBones()+1);
   for (uint i=0; i<joints.size(); ++i) 
      joints[i] = s.getJointPos(i);
   
   closest_vertex.resize(nBones()+1, -1);
   min_distance_vertex.resize(nBones()+1, 10e8);
   onlyvisible.resize(nBones()+1, true);
   d.resize(nBones()+1);
   k.resize(nBones()+1, false);
   kdist.resize(nBones()+1, 0.0);
   visited.resize(nBones()+1, false);
   D.resize(nBones()+1, 0.0);
   relatedParam.resize(joints.size(), make_pair(0.0, -1));
   
   limits(); // compute the center and radius of bounded sphere
   buildDistanceBoneTable();
   genControllersAndArtificialBones();
}

GSkeleton::GSkeleton(const GSkeleton &s) 
   :Skeleton(s) {
   
   graph = s.graph;
   joints = s.joints;
   bones = s.bones;
   branches = s.branches;
   prev = s.prev;
   dBonesTable = s.dBonesTable;
   filename = s.filename;
   k = s.k;
   kdist  = s.kdist;
   visited = s.visited;
   D = s.D;
   dBonesTable = s.dBonesTable;
   controller = s.controller;
   atbones = s.atbones;
   orderList = s.orderList;
   
   d.resize(nJoints());
   limits(); // compute the center and radius of bounded sphere
   closest_vertex = s.closest_vertex;
   min_distance_vertex = s.min_distance_vertex;
   onlyvisible = s.onlyvisible;
   relatedParam = s.relatedParam;
}

GSkeleton::GSkeleton(const vector<Point3> &j, const vector<vector<int> > &g) {
   
   joints = j;
   graph = g;
   genBonesList();
   
   closest_vertex.resize(nBones()+1, -1);
   min_distance_vertex.resize(nBones()+1, 10e8);
   onlyvisible.resize(nBones()+1, true);
   d.resize(nBones()+1);
   k.resize(nBones()+1, false);
   kdist.resize(nBones()+1, 0.0);
   visited.resize(nBones()+1, false);
   D.resize(nBones()+1, 0.0);
   relatedParam.resize(joints.size(), make_pair(0.0, -1));
   limits(); // compute the center and radius of bounded sphere
   
   buildDistanceBoneTable();
   genControllersAndArtificialBones();
}

GSkeleton::~GSkeleton() {}

void GSkeleton::genBonesList() {
   
   genBranches();
   
   /// bones in PREV format generation
   prev.resize(graph.size(), -1); // graph.size() == nJoints(), however nJoints() can't be present yet
   for (uint i=0; i<branches.size(); ++i) {
      const vector<int> &branch = branches[i];
      for (uint j=1; j<branch.size(); ++j) {
         const int &idprev = branch[j-1];
         const int &id = branch[j];
         prev[id] = idprev;
      }
   }
   
   /// bones in NORMAL format generation
   for (int i=0; i<prev.size(); ++i) {
      if (prev[i] < 0) continue;
      bones.push_back(make_pair(i, prev[i]));
   }
   
   assert(prev.size() == bones.size()+1);
   
   cout<<"the bones ...."<<bones.size()<<endl;
   printBones();
   cout<<"the bones .... in prev format"<<prev.size()<<endl;
   printBonesPREV();
   cout<<"the bones ...."<<branches.size()<<endl;
   printBranches();
   
   //~ // using a map to avoid duplicated keys on the bones list
   //~ map<pair<int, int>, int > bonestmp;
   //~ for (uint f=0; f<graph.size(); ++f) {
      //~ const vector<int> &target = graph[f];
      //~ for (uint c=0; c<target.size(); ++c) {
         //~ uint cs = (f<target[c])? f : target[c];
         //~ uint ct = (f<target[c])? target[c] : f;
         //~ pair<int, int> pair(cs, ct);
         //~ bonestmp.insert(make_pair(pair, 0));
      //~ }
   //~ }
   
   //~ bones.clear();
   //~ map<pair<int, int>, int >::iterator it = bonestmp.begin();
   //~ while(it != bonestmp.end()) {
      //~ const pair<int, int> &b = it->first;
      //~ bones.push_back(b);
      //~ ++it;
   //~ }
}

void GSkeleton::genControllersAndArtificialBones() {
   
   cout<<"genControllersAndArtificialBones() ...."<<endl;
   vector<vector<int> > &c = controller;
   c.resize(nBones());
   
   /// female.ske
   //~ c[0].resize(1);   c[0][0] = -1;
   //~ c[1].resize(1);   c[1][0] = 0;
   //~ c[2].resize(1);   c[2][0] = 7;
   //~ c[3].resize(1);   c[3][0] = 4;
   //~ c[4].resize(1);   c[4][0] = 3;
   //~ c[5].resize(2);   c[5][0] = 4;  c[5][1] = 6;
   //~ c[6].resize(1);   c[6][0] = 9;
   //~ c[7].resize(2);   c[7][0] = 2; c[7][1] = 10;
   //~ c[8].resize(2);   c[8][0] = 2; c[8][1] = 10;
   //~ c[9].resize(1);   c[9][0] = 6;
   //~ c[10].resize(1);  c[10][0] = 7;
   //~ c[11].resize(2);  c[11][0] = 9; c[11][1] = 12;
   //~ c[12].resize(1);  c[12][0] = 13;
   //~ c[13].resize(1);  c[13][0] = 12;
   //~ c[14].resize(1);  c[14][0] = 15;
   //~ c[15].resize(1);  c[15][0] = -1;
   //~ c[16].resize(1);  c[16][0] = 8;
   //~ c[17].resize(2);  c[17][0] = 6; c[17][1] = 9;
   
   
   /// female_01.ske
   c[0].resize(1);   c[0][0] = 20;
   c[1].resize(1);   c[1][0] = 19;
   c[2].resize(2);   c[2][0] = 4; c[2][1] = 9;
   c[3].resize(1);   c[3][0] = 9;
   c[4].resize(1);   c[4][0] = 21;
   c[5].resize(1);   c[5][0] = 8;
   c[6].resize(2);   c[6][0] = 19; c[6][1] = 21;
   c[7].resize(2);   c[7][0] = 1; c[7][1] = 11;
   c[8].resize(1);   c[8][0] = 19;
   c[9].resize(1);   c[9][0] = 3;
   c[10].resize(1);  c[10][0] = 19;
   c[11].resize(1);  c[11][0] = 19;
   c[12].resize(2);  c[12][0] = 17; c[12][1] = 14;
   c[13].resize(1);  c[13][0] = 17;
   c[14].resize(1);  c[14][0] = 21;
   c[15].resize(1);  c[15][0] = 16;
   c[16].resize(1);  c[16][0] = 19;
   c[17].resize(1);  c[17][0] = 13;
   c[18].resize(1);  c[18][0] = 19;
   c[19].resize(2);  c[19][0] = 1; c[19][1] = 11;
   c[20].resize(1);  c[20][0] = 7;
   c[21].resize(2);  c[21][0] = 14; c[21][1] = 4;
   
   /// artificial bones
   atbones.resize(4*nBones(), Vector3(0,0,0));
   const float LIMINF = 1e-1;
   for (int i=0; i<nBones(); ++i) {
      const pair<int, int> &bi = bones[i];
      Vector3 unibi = getJointPos(bi.first)-getJointPos(bi.second);
      unibi = unibi/sqrt(unibi*unibi);
      
      for (int j=0; j<c[i].size(); ++j) {
         if (c[i][j] == -1) break;
         Number d = getDistanceBetweenBone(i,c[i][j]);
         bool flag = false;
         int k;
         if (d==0) {
            int l = c[i][j];
            for (int m=0; m<c[l].size(); ++m) {
               if (c[l][m] == i) {
                  flag = true;
               }
            }
            if (flag) break;
            else k = c[i][j];
         } else {
            k = -1; /// ???
            do {
               k++;
            }
            while ((getDistanceBetweenBone(i,k)>0) or k==i or 
                   (getDistanceBetweenBone(c[i][j],k)>=d));
            assert(k>=0);
         }
         
         const Segment3 &bk = getBoneSegment(k);
         Vector3 unibk = bk[0] - bk[1];
         unibk = unibk/sqrt(unibk*unibk);
         
         Vector3 xprod = cross_product(unibi,unibk);
         if (xprod*xprod < LIMINF) {
            xprod = cross_product(unibi,Vector3(0,0,1));
         }
         if (xprod*xprod < LIMINF) {
            xprod = cross_product(unibi,Vector3(0,1,0));
         }
         
         int k0 = k;
         while (atbones[k0] != Vector3(0,0,0)) {
            k0 += nBones();
         }
         c[i][j] = k0 + nBones();
         atbones[k0] = xprod;
      }
   }
   
   //~ cout<<"controllers ..."<<endl;
   //~ for (int i=0; i<controller.size(); ++i) {
      //~ const vector<int> &con = controller[i];
      //~ cout<<"c: "<<i<<" : ";
      //~ for (int j=0; j<con.size(); ++j) {
         //~ cout<<con[j]<<" ";
      //~ }
      //~ cout<<endl;
   //~ }
   
   //~ cout<<"artificial bones"<<endl;
   //~ for (int i=0; i<atbones.size(); ++i) {
      //~ cout<<"ab: "<<i<<" : "<<atbones[i]<<endl;
   //~ }
   
   //~ cout<<"order list generation..."<<endl;
   /// order list generation
   orderList.resize(nBones());
   vector<int> posOrderList(nBones());
   bool exchangeflag = false; int k = 0;
   for (unsigned int i=0; i<nBones(); ++i) posOrderList[i] = i;
   
   do {
      exchangeflag = false;
      k++;
      for (unsigned int i=0; i<bones.size(); ++i) {
         for (unsigned int j=0; j<controller[i].size(); ++j) {
            int ctemp;
            if (controller[i][j] > nBones()) {
               ctemp = controller[i][j] % nBones();
               if (posOrderList[ctemp]>posOrderList[i]) {
                  int tmp = posOrderList[i];
                  posOrderList[i] = posOrderList[ctemp];
                  posOrderList[ctemp] = tmp;
                  exchangeflag = true;
               }
            }
         }
      }
   } while ((exchangeflag) and k < nBones());
   
   for (unsigned int i=0; i<orderList.size(); ++i) {
      int j = posOrderList[i];
      orderList[j] = i;
   }
   
   //~ cout<<"ordered list"<<endl;
   //~ for (int i=0; i<orderList.size(); ++i) {
      //~ cout<<"ol: "<<i<<" : "<<orderList[i]<<endl;
   //~ }
   
}

Vector3 GSkeleton::getBoneProjector(const Point3 &v, const int idbone) {
   
   const Segment3 &bo = getBoneSegment(idbone);
   const Point3 &a = bo.source();
   const Point3 &b = bo.target();
   
   const Vector3 &ba = b-a;
   const Vector3 &va = v-a;
   const Vector3 &vb = v-b;
   
   const Number &norma2aibi = ba*ba;
   const Number &dotvaibiai = va*ba;
   
   if (dotvaibiai<=0)                  { return va; }
   else if (dotvaibiai>=norma2aibi)    { return vb; }
   else {
      Number t = dotvaibiai/norma2aibi;
      return (Vector3(v[0],v[1],v[2]) - (Vector3(b[0],b[1],b[2])*t + Vector3(a[0],a[1],a[2])*(1.0-t)));
   }
}

float GSkeleton::getDistance(const Point3 &v, const int idbone, float &t) {
   
   const Segment3 &bo = getBoneSegment(idbone);
   const Point3 &a = bo.source();
   const Point3 &b = bo.target();
   
   const Vector3 &ba = b-a;
   const Vector3 &va = v-a;
   const Vector3 &vb = v-b;
   
   const Number &norma2aibi = ba*ba;
   const Number &dotvaibiai = va*ba;
   
   Number dminvaibi;
   if (dotvaibiai<=0)                  { dminvaibi = sqrt(va*va); t=1.0;}
   else if (dotvaibiai>=norma2aibi)    { dminvaibi = sqrt(vb*vb); t=0.0;}
   else                                { 
      dminvaibi = sqrt(va*va - ((dotvaibiai*dotvaibiai)/norma2aibi)); 
      t = 1.0-dotvaibiai/norma2aibi;
   }
   
   return dminvaibi;
}

void GSkeleton::limits() {
   /*
   map<string, Bone>::iterator it = bones.begin();
   Number mincoo[3], maxcoo[3];
   Point3 p = point(it->second.getName());
   mincoo[0]=maxcoo[0]=p[0]; mincoo[1]=maxcoo[1]=p[1]; mincoo[2]=maxcoo[2]=p[2];
   while (it != bones.end()) {
      string b = it->second.getName();
      Point3 p = point(b);
      for (int i = 0; i < 3; ++i) {
         if (p[i] < mincoo[i])      mincoo[i] = p[i];
         else if (p[i] > maxcoo[i]) maxcoo[i] = p[i];
      }
      ++it;
   }
   
   Point3 WorldpMin(mincoo[0],mincoo[1],mincoo[2]);
   Point3 WorldpMax(maxcoo[0],maxcoo[1],maxcoo[2]);
   Vector3 size(WorldpMin, WorldpMax);
   size = size*0.5;
   _center = WorldpMin + size;
   _radius = sqrt(size * size); // vector lenght
   */
}

const Segment3 GSkeleton::getBoneSegment(int i) const {
   const pair<int, int> &bone = bones[i];
   const Point3 &a = joints[bone.first];
   const Point3 &b = joints[bone.second];
   return Segment3(a,b);
}

Point3 GSkeleton::getJointPos(int i) {
   return joints[i];
}

void GSkeleton::setJointPos(int i, const Point3 &q) {
   joints[i] = q;
   //~ const Point3 &p = getJointPos(i);
   //~ translateJoint(i, q-p, 'L');
}

void GSkeleton::translateJoint(int i, const Vector3 &t, char mode) {
   
   /// mode == 'T' : hierarchical translation
   /// mode == 'R' : hierarchical rotation
   /// mode == 'L' : local translation
   /*
   if (i > nBones() or i < 0) return; // invalid joint
   else if (i == nBones()) { // i is the root
      Point3 p = getRootPosition();
      setRootPosition(p+t);
      
      if (mode == 'L') {
         /// translate the vector of bone's sons
         vector<string> &sons = hierarchy["root"];
         vector<string>::iterator it = sons.begin();
         while (it != sons.end()) {
            string name = *it++;
            Bone &son = bones[name];
            
            Vector3 vson = son.lenXdir(); // get the vector of bone
            vson = vson - t;
            Number lson = sqrt(vson*vson);
            son.setLength(lson);
            son.setDirection(vson/lson);
         }
      }
      else if (mode == 'T') {
         // nothing is ok
      }
      else if (mode == 'R') {
      
      }
   }
   else {
      string namebone = boneName(i); // name of the bone
      Bone &bone = bones[namebone];
      
      /// translate the vector of bone
      Vector3 boneDirOri = bone.getDirection(); // get the direction vector of bone (used for computing the quaternion)
      Vector3 vbone = bone.lenXdir(); // get the vector of bone
      vbone = vbone + t;
      Number lbone = sqrt(vbone*vbone);
      bone.setLength(lbone);
      bone.setDirection(vbone/lbone);
      
      Number w=1.0, x=0.0, y=0.0, z=0.0; // the identity quaternion
      Number cosa = boneDirOri*bone.getDirection();
      Vector3 cross = cross_product(boneDirOri, bone.getDirection());
      if (cross*cross > 1e-6) {
         float l = sqrt(cross*cross + cosa*cosa);
         w=cosa; x=cross[0]/l; y=cross[1]/l; z=cross[2]/l;
      }
      Quat quaternion(w,x,y,z);
      
      if (mode == 'L') {
         /// translate the vector of bone's sons
         vector<string> &sons = hierarchy[namebone];
         vector<string>::iterator it = sons.begin();
         while (it != sons.end()) {
            string name = *it++;
            Bone &son = bones[name];
            
            Vector3 vson = son.lenXdir(); // get the vector of bone
            vson = vson - t;
            Number lson = sqrt(vson*vson);
            son.setLength(lson);
            son.setDirection(vson/lson);
         }
      }
      else if (mode == 'T') { 
         // nothing is ok
      }
      else if (mode == 'R') {
         /// transform the son's hierarchy 
         queue<string> Q;
         Q.push(namebone);
         while (not Q.empty()) {
            string name = Q.front();
            Q.pop();
            
            Bone &son = bones[name];
            
            Vector3 vson = son.getDirection(); // get the vector of bone
            /// rotation computation
            //~ Quat qPo(vson[0],vson[1],vson[2]);
            //~ Quat qp = Q*qPo*conjugate(Q);
            son.setDirection(vson);
            
            Sons sons = getSons(name);
            for (int i = 0; i < (int)sons.size(); i++)
               Q.push(sons[i]);
         }
      }
   }
   */
}

// This struct is used to compute the dijkstra algorithm
struct Joint_id_d {
   
   Joint_id_d(): id(0), d(0) {}
   Joint_id_d(int i, Number di): id(i), d(di) {}
   
   int     id;    // id of skeleton's joint
   Number   d;    // distance value on vertex id-th
};

// functor to compare skeleton's joints 
struct compareJointDistance {
   bool operator() (const Joint_id_d &a, const Joint_id_d &b) const {
      return a.d > b.d;
   }
};

void GSkeleton::floodfill_Dijkstra(int vindex, bool kdistFlag) {
   
    cout<<"floodfill_Dijkstra ... "<<endl;
   /// initializing 
   for (int i=0; i<=nBones(); ++i) d[i] = NINFINITY;
   
   d[vindex] = 0;
   priority_queue<Joint_id_d, vector<Joint_id_d>, compareJointDistance>   priority_queue;
   priority_queue.push(Joint_id_d(vindex, d[vindex]));
   
   while (not priority_queue.empty()) {
      Joint_id_d f = priority_queue.top();
      priority_queue.pop();
      
      const vector<int> &target = graph[f.id];
      const Point3 &pf = getJointPos(f.id);
      for (unsigned int c=0; c<target.size(); ++c) {
         const Point3 &pc = getJointPos(target[c]);
         const Vector3 fc(pf[0]-pc[0], pf[1]-pc[1], pf[2]-pc[2]);
         Number dfc = sqrt(fc*fc);
         //old code
         //if (k[f.id]) dfc = 100000;
         //else         dfc = sqrt(fc*fc);
         
         if (d[f.id] + dfc < d[target[c]]) {
            //d[target[c]] = d[f.id] + dfc + (kdistFlag? (f.id == vindex? 0.0 : kdist[f.id]) : 0);
            //~ d[target[c]] = d[f.id] + dfc + (kdistFlag? (kdist[f.id]) : 0);
            //~ priority_queue.push(V_id_d (target[c], d[target[c]]));
            
            float tmp = d[f.id] + dfc + (kdistFlag? kdist[target[c]] : 0);
            d[target[c]] = tmp;
            priority_queue.push(Joint_id_d (target[c], tmp));
         }
         //old code
         //if (d[f.id] + dfc < d[target[c]]) {
            //d[target[c]] = d[f.id] + dfc;
            //priority_queue.push(Joint_id_d (target[c], d[target[c]]));
         //}
      }
   }
}

GSkeleton* GSkeleton::load(const string &filename) {
   
   if (filename.empty()) {
      cout<<"noname file!"<<endl; 
      return 0;
   }
   string extension = filename.substr(filename.size() - 4, 4);
   if (extension == ".ske") {
      std::fstream skel_file(filename.c_str(), std::ios_base::in);
      if (skel_file) {
         GSkeleton *s = new GSkeleton;
         s->import_from_ske(filename.c_str());
         s->filename = filename;
         return s;
      } else {
         cout<<"file not found!"<<endl;
         //assert(false and "unknowed extension!");
         return 0;
      }
   } else {
      cout<<"unknowed extension!"<<endl;
      //assert(false and "unknowed extension!");
      return 0;
   }
}

void GSkeleton::import_from_ske(const char *fname) {
   
   string filename(fname);
   if (filename.empty()) return;
   if (filename.substr(filename.size() - 4, 4) != ".ske") { assert(false and "unknowed extension!"); }
   
   ifstream skelfile(filename.c_str());
   if (not skelfile.is_open()) {
      cout<<"I/O error: grrr the skeleton file doesn't exist "<<endl;
      assert(false);
   }
   
   string skelformat; int njoints;
   // Load header
   skelfile>>skelformat;
   assert(skelformat=="SKE");
   skelfile>>njoints;
   
   // Load the joints positions
   Number x, y, z; int id, n;
   for (int i=0; i<njoints; ++i) {
      skelfile>>x>>y>>z;
      joints.push_back(Point3(x,y,z));
   }
   // Load the graph
   graph.resize(njoints);
   for (int i=0; i<njoints; ++i) {
      skelfile>>n;
      for (int j=0; j<n; ++j) {
         skelfile>>id;
         graph[i].push_back(id);
      }
   }
   
   // Generate the list of bones
   genBonesList();
   // Initialize auxiliar structures
   closest_vertex.resize(nBones()+1, -1);
   min_distance_vertex.resize(nBones()+1, 10e8);
   onlyvisible.resize(nBones()+1, true);
   d.resize(nBones()+1);
   k.resize(nBones()+1, false);
   kdist.resize(nBones()+1, 0.0);
   D.resize(nBones()+1, 0.0);
   visited.resize(nBones()+1, false);
   relatedParam.resize(joints.size(), make_pair(0.0, -1));
   limits(); // compute the center and radius of bounded sphere
   buildDistanceBoneTable();
   //~ genControllersAndArtificialBones();
}

void GSkeleton::export_to_ske(const char *fname) {
   
   string filename(fname);
   if (filename.empty()) return;
   if (filename.substr(filename.size() - 4, 4) != ".ske") { assert(false and "unknowed extension!"); }
   ofstream fileout(filename.c_str());
   if (fileout) {
      fileout<<"SKE"<<endl;
      fileout<<nJoints()<<endl;
      for (uint i=0; i<nJoints(); ++i)
         fileout<<getJointPos(i)<<endl;
      
      for (uint i=0; i<graph.size(); ++i) {
         const vector<int> &neig = graph[i];
         fileout<<neig.size()<<" ";
         for (uint j=0; j<neig.size(); ++j)
            fileout<<neig[j]<<" ";
         fileout<<endl;
      }
   }
   fileout.close();
}

void GSkeleton::buildDistanceBoneTable() {
   
   assert(nBones() > 0);
   cout<<"computeBonesDistanceTable(): ... begin"<<endl;
   dBonesTable.resize(nBones());
   for (unsigned int i=0; i<dBonesTable.size(); ++i) {
      dBonesTable[i].resize(nBones(), 0.0);
   }
   
   for (unsigned int i=0; i<nBones(); ++i) {
      const pair<int, int> &bi = bones[i];   // initial bone
      floodfill_Dijkstra(bi.first);
      vector<Number> dist = d;
      for (unsigned int j=0; j<nBones(); ++j) {
         if (i == j) continue;
         const pair<int, int> &bj = bones[j];   // initial bone
         if (d[bj.first] < d[bj.second])
            dBonesTable[i][j] = d[bj.first];
         else 
            dBonesTable[i][j] = d[bj.second];
      }
      floodfill_Dijkstra(bi.second);
      for (unsigned int j=0; j<nBones(); ++j) {
         if (i == j) continue;
         const pair<int, int> &bj = bones[j];   // initial bone
         if (d[bj.first] < d[bj.second])
            dBonesTable[i][j] = min(dBonesTable[i][j], d[bj.first]);
         else 
            dBonesTable[i][j] = min(dBonesTable[i][j], d[bj.second]);
      }
   }
   
   //~ for (unsigned int i=0; i<dbones.size(); ++i) {
      //~ for (unsigned int j=0; j<dbones.size(); ++j) {
         //~ cout<<"("<<i<<","<<j<<")"<<dbones[i][j]<<" ";
      //~ }
      //~ cout<<endl;
   //~ }
   cout<<"computeBonesDistanceTable(): end"<<endl;
}

void GSkeleton::genBranches() {
   
   assert(graph.size() > 0);
   ismarked.resize(graph.size(), false);
   for (int i=0; i<graph.size(); ++i) {
      assert(not graph[i].empty());
      if (graph[i].size() < 3) continue; // find the seed
      const vector<int> &neig = graph[i];
      for (uint j=0; j<neig.size(); ++j) {
         branch.clear();
         branch.push_back(i);
         ismarked[i] = true;
         getBranch(neig[j]);
         if(branch.size() > 1) branches.push_back(branch);
      }
   }
   
   // only one branch case
   if (branches.empty() and not graph.empty()) {
      int i = 0;
      const vector<int> &neig = graph[i];
      for (uint j=0; j<neig.size(); ++j) {
         branch.clear();
         branch.push_back(i);
         ismarked[i] = true;
         getBranch(neig[j]);
         if(branch.size() > 1) branches.push_back(branch);
      }
   }
}

void GSkeleton::getBranch(int id) {
   
   if (ismarked[id]) return;
   if (isTerminal(id) or isJunction(id)) {
      branch.push_back(id);
      ismarked[id] = true;
   } else {
      branch.push_back(id);
      ismarked[id] = true;
      const vector<int> &neig = graph[id];
      for (uint j=0; j<neig.size(); ++j) {
         if (ismarked[neig[j]]) continue;
         getBranch(neig[j]);
      }
   }
}

void GSkeleton::printBones() {
   
   for (int i=0; i<bones.size(); ++i) {
      const pair<int, int> &bone = bones[i];
      cout<<i<<": "<<bone.first<<" "<<bone.second<<endl;
   }
}

void GSkeleton::printBranches() {
   
   for (uint i=0; i<branches.size(); ++i) {
      cout<<i<<" : " ;
      copy(branches[i].begin(), branches[i].end(), ostream_iterator<int>(cout, " "));
      cout<<endl;
   }
}

void GSkeleton::printBonesPREV() {
   
   for (uint i=0; i<prev.size(); ++i) {
      cout<<i<<" : "<<prev[i]<<endl;
   }
}

void GSkeleton::convert2DualQuat() {
   
   dualQuats.resize(transformation.size());
   for (uint i=0; i<transformation.size(); ++i) {
      const OMR::Vector3 &t = transformation[i].getTrans();
      const OMR::Quaternion<> &Q = transformation[i].getRot();
      OMR::Quaternion<> tQ(0,t[0],t[1],t[2]);
      dualQuats[i] = tQ*Q;
   }
}

Number GSkeleton::pathLength(int s, int t) {
   
    //~ cout<<"pathLength_Dijkstra ... "<<endl;
   /// initializing 
   for (int i=0; i<=nBones(); ++i) d[i] = NINFINITY;
   
   d[s] = 0;
   priority_queue<Joint_id_d, vector<Joint_id_d>, compareJointDistance>   priority_queue;
   priority_queue.push(Joint_id_d(s, d[0]));
   
   while (not priority_queue.empty()) {
      Joint_id_d f = priority_queue.top();
      priority_queue.pop();
      
      if (f.id == t) break;
      const vector<int> &target = graph[f.id];
      const Point3 &pf = getJointPos(f.id);
      for (unsigned int c=0; c<target.size(); ++c) {
         const Point3 &pc = getJointPos(target[c]);
         const Vector3 fc(pf[0]-pc[0], pf[1]-pc[1], pf[2]-pc[2]);
         Number dfc = sqrt(fc*fc);
         
         if (d[f.id] + dfc < d[target[c]]) {
            d[target[c]] = d[f.id] + dfc;
            priority_queue.push(Joint_id_d (target[c], d[target[c]]));
         }
      }
   }

   return d[t];
}

vector<unsigned> GSkeleton::path(int s, int t) {

   if (s == t) {
      vector<unsigned> p(1,s);
      return p;
   }

   vector<int> prevtmp(nJoints(), -1);
   for (int i=0; i<=nBones(); ++i) d[i] = NINFINITY;
   
   d[s] = 0;
   priority_queue<Joint_id_d, vector<Joint_id_d>, compareJointDistance>   priority_queue;
   priority_queue.push(Joint_id_d(s, d[0]));
   
   while (not priority_queue.empty()) {
      Joint_id_d f = priority_queue.top();
      priority_queue.pop();
      
      if (f.id == t) break;
      const vector<int> &target = graph[f.id];
      const Point3 &pf = getJointPos(f.id);
      for (unsigned int c=0; c<target.size(); ++c) {
         const Point3 &pc = getJointPos(target[c]);
         const Vector3 fc(pf[0]-pc[0], pf[1]-pc[1], pf[2]-pc[2]);
         Number dfc = sqrt(fc*fc);
         
         if (d[f.id] + dfc < d[target[c]]) {
            d[target[c]] = d[f.id] + dfc;
            priority_queue.push(Joint_id_d (target[c], d[target[c]]));
            prevtmp[target[c]] = f.id;
         }
      }
   }
   
   vector<unsigned> p;
   int idx = t;
   do {
      p.push_back(idx);
      idx = prevtmp[idx];
   } while (idx != s);
   p.push_back(s);
   reverse(p.begin(), p.end());
   
   return p;
}

